#include "SimpleSerial.cpp"
#include "mower_platform.cpp"
#include <iostream>
#include <cstdio>
#include <ros/ros.h>
#include <ros/console.h>
#include <string>
#include <iostream>
#include <time.h>
#include <fstream>
#include <stdlib.h>
#include <math.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_msgs/UInt32.h>
using namespace std;

#define DEBUGGING true
#define CALIBRATED_PI_RIGHT 2.727 //was 2.72
#define CALIBRATED_PI_LEFT 2.727 //was 2.72
#define ADDITIONAL_PHASE_THREE_CORRECTION 0.07
#define MOWING_LENGTH 15.0
#define MOWING_WIDTH 3.0
#define BLADE_WIDTH .5
#define K_PID 1500
#define PID_FREQUENCY 3
#define FRAME_ANGLE_OFFSET -2.6 //degrees, will be converted

class SensorDemo {
public:
  SensorDemo(ros::NodeHandle nh_, char *arduino_path)
      : n(nh_),
	currentState(STOPPED),
	currentPhase(PHASEONE),
	turnsMade(0),
	yaw_global_set(false),
        demoMower(arduino_path, 9600),
        obstacle_detected_phase_one(false),
        obstacle_detected_phase_three(false),
	entered_last_stage(false) {
    std::cout << "subscribing now... \n";
    encoder_sub = n.subscribe("odom", 10, &SensorDemo::PoseCallback, this);
    obstacle_sub = n.subscribe("stop_signal", 10, &SensorDemo::ObstacleCallback, this);
  }

  void CalculateNewEstimatedYaw() {
    if (currentPhase == PHASEONE || currentPhase == PHASETWO) { //making right turns
      estimated_current_yaw += 3.1415926/2;
      if (currentPhase == PHASETWO) {
	estimated_current_yaw += (FRAME_ANGLE_OFFSET * 3.14159)/180;
      }
      if (estimated_current_yaw > 3.1415926) {
	double remainder = estimated_current_yaw - 3.1415926;
	estimated_current_yaw = -(3.1415926) + remainder;
      }
    } else { //making left turns
      if (currentPhase == PHASETHREE) {
	estimated_current_yaw -= (FRAME_ANGLE_OFFSET * 3.14159)/180;
      }
      estimated_current_yaw -= 3.1415926/2;
      if (estimated_current_yaw < -3.1415926) {
	double remainder = estimated_current_yaw + 3.1415926;
	estimated_current_yaw = (3.1415926) + remainder;
      }
    }
    ROS_INFO("Calculating new estimated yaw: %f", estimated_current_yaw);
  }

  void SetUpGlobalTurningGoals(const nav_msgs::Odometry &msg) {
    geometry_msgs::Vector3 imuVector = QuaternionToYpr(msg);
    yaw_forward = imuVector.z;
    estimated_current_yaw = yaw_forward;
    absolute_beginning_yaw = estimated_current_yaw;
    ROS_INFO("Setting global turn goals: %f", estimated_current_yaw);

  }

  double CalculateYawDeviation(double actual_yaw) {
    double deviation = 0;
    if (currentState == TURNING) {
      if (currentPhase == PHASEONE || currentPhase == PHASETWO) { //making right turns
	deviation = actual_yaw - estimated_current_yaw;
      } else { //making left turns
	deviation = estimated_current_yaw - actual_yaw;
      }
      if (deviation < -(3.1415926)) {
	deviation += (2*3.1415926);
      }
    } else if (currentState == DRIVING) {
      deviation = actual_yaw - estimated_current_yaw;
    }
    return deviation;
  }

  void IncreaseWidthTravelled() {
    width_travelled+=BLADE_WIDTH;
  }

  void RightTurn(const nav_msgs::Odometry &msg, int speed, int turn_count) {
    geometry_msgs::Vector3 imuVector = QuaternionToYpr(msg);
    if (IsImuDataCleared()) {
      SetImuData(imuVector.z);
      demoMower.ControlledTurnRight(speed);
      yaw_goal = CALIBRATED_PI_RIGHT/2 - CalculateYawDeviation(imuVector.z); 
      ROS_INFO("dev = %f yaw_goal = %f", CalculateYawDeviation(imuVector.z), yaw_goal);
    }
    if (initial_yaw + (yaw_goal) > (3.14)) {
      double remainder = 3.14 - initial_yaw; 
      if (imuVector.z < 0){
	imuVector.z += 3.14;
      } else {
	imuVector.z = 0;
      }
      if (imuVector.z + remainder > (yaw_goal)) {
	demoMower.stop();  // stop the mower
	turnsMade++;
	CalculateNewEstimatedYaw();
	ChangePhase();
	ResetImuData();
	ResetEncoderData();
	currentState = DRIVING;
      } 
    } else { 
      if (CalculateAmountTurned(imuVector.z) > (yaw_goal)) {
	demoMower.stop();  // stop the mower
	CalculateNewEstimatedYaw();
	ChangePhase();
	turnsMade++; 
	ResetImuData();
	ResetEncoderData();
	currentState = DRIVING;
      }
    }
      if (turnsMade >= turn_count) {
	//Need to resume driving
	demoMower.stop();
	ChangePhase();
	ResetEncoderData();
	currentState = DRIVING;
    }
  }

  void LeftTurn(const nav_msgs::Odometry &msg, int speed, int turn_count) {
    geometry_msgs::Vector3 imuVector = QuaternionToYpr(msg);
    if (IsImuDataCleared()) {
      SetImuData(imuVector.z);
      demoMower.ControlledTurnLeft(speed);
      yaw_goal = CALIBRATED_PI_RIGHT/2 - CalculateYawDeviation(imuVector.z); 
      ROS_INFO("LEFT TURN deviation = %f", CalculateYawDeviation(imuVector.z));
    }
    if (initial_yaw - (yaw_goal) < (-3.14)) {
      double remainder = -3.14 - initial_yaw;
      if (imuVector.z > 0){
	imuVector.z -= 3.14;
      } else {
	imuVector.z = 0;
      }
      if (imuVector.z + remainder < (-yaw_goal)) {
	ROS_INFO("curyaw = %f, remainder = %f, yawgoal=%f", imuVector.z, remainder, yaw_goal);
	demoMower.stop();  // stop the mower
	ROS_INFO("GOT INSIDE OF 1");
	turnsMade++; //Whenever we inc this, we have completed a 90 deg turn
	CalculateNewEstimatedYaw();
	ChangePhase();
	ResetImuData();
	ResetEncoderData();
	currentState = DRIVING;
      }
    } else { 
      if (CalculateAmountTurned(imuVector.z) < (-yaw_goal)) {
	ROS_INFO("GOT INSIDE OF 2");
	demoMower.stop();  // stop the mower
	CalculateNewEstimatedYaw();
	ChangePhase();
	turnsMade++; 
	ResetImuData();    
	ResetEncoderData();
      	currentState = DRIVING; 
      }
    }
    if (turnsMade >= turn_count) {
      ROS_INFO("GOT INSIDE OF 3");
      //Need to resume driving
      demoMower.stop();
      ChangePhase();
      ROS_INFO("Ending turn at yaw %f", imuVector.z);
      ResetEncoderData();
      currentState = DRIVING;
    }
  }

  void ChangePhase()
  {
    // dont bother changing current state; the turns manage it.  Any change to state
    // in this method will be overwritten!
    system("sleep 1");
    ROS_INFO("CHANGING PHASE! width_traversed: %f width goal: %f", width_travelled, MOWING_WIDTH);
    if (currentState == TRANSITION_LEFT) {
      currentPhase = PHASETHREE;
      // at this point, we've already turned 180 deg.  We need to set the phase1 yaw
      // to be behind us.
      absolute_beginning_yaw = absolute_beginning_yaw + (CALIBRATED_PI_RIGHT);
      ROS_INFO("CHANGED PHASE ONE BEARING TO %f", absolute_beginning_yaw);
      return; //RETURN, DONT FALL THROUGH!
    } else if (currentState == TRANSITION_RIGHT) {
      currentPhase = PHASEONE;
      // at this point, we've already turned 180 deg.  We need to set the phase1 yaw
      // to be behind us.
      absolute_beginning_yaw = absolute_beginning_yaw + (CALIBRATED_PI_LEFT);
      ROS_INFO("CHANGED PHASE ONE BEARING TO %f", absolute_beginning_yaw);
      return; //RETURN, DONT FALL THROUGH!
    }

    if (currentPhase == PHASEONE) {
      IncreaseWidthTravelled();
      currentPhase = PHASETWO;
    } else if (currentPhase == PHASETWO) {
      currentPhase = PHASETHREE; 
    } else if(currentPhase == PHASETHREE) {
      IncreaseWidthTravelled();
      currentPhase = PHASEFOUR;
    } else if(currentPhase == PHASEFOUR) {
      currentPhase = PHASEONE;
    }
  }

  void DriveStraight(const nav_msgs::Odometry &msg) {
      ekf_callback_counter++;
      ResetImuData();
      if (IsEncoderDataCleared()) {
	SetEncoderData(msg.pose.pose.position.x, msg.pose.pose.position.y);
	ROS_INFO("Setting my straight yaw to %f", estimated_current_yaw); 
	demoMower.ControlledMove(1300);
	if (currentPhase == PHASEONE) {
	  estimated_current_yaw = absolute_beginning_yaw;
	}
	if (entered_last_stage) {
	  ROS_INFO("We're in the last stage");
	  if (currentPhase == PHASEONE || currentPhase == PHASETHREE) {
	    distance_to_travel = MOWING_WIDTH;
	  } else {
	    distance_to_travel = MOWING_LENGTH;
	  }
	} else {
	  if (currentPhase == PHASEONE || currentPhase == PHASETHREE) {
	    distance_to_travel = MOWING_LENGTH; //meters
	    ROS_INFO("Setting distance to travel to %f", distance_to_travel);
	    if(obstacle_detected) {
	      distance_to_travel = distance_to_obstacle;
	      obstacle_detected = false;
	    }
	  } else if (currentPhase == PHASETWO || currentPhase == PHASEFOUR) {
	    distance_to_travel = BLADE_WIDTH;
	  }
	}
      }
      last_known_x = msg.pose.pose.position.x;
      last_known_y = msg.pose.pose.position.y;
      // check to see if we've arrived at the proper location
      if (CalculateDistanceTravelled(msg.pose.pose.position.x,
				     msg.pose.pose.position.y) > distance_to_travel) {
	ROS_INFO("Destination Reached!");
	demoMower.ControlledMove(1000);
	ResetImuData();
	ekf_callback_counter = 0;
	// entry point to mowing behind obstacle perra
	if ((currentPhase == PHASEONE || currentPhase == PHASETHREE) &&
	    width_travelled >= MOWING_WIDTH && !entered_last_stage) {
	  entered_last_stage = true;
	  ROS_INFO("Setting entered_last_stage to true");
	  if (obstacle_detected_phase_one && currentPhase == PHASEONE) {
	    //	    ROS_INFO("************STOPPIN");
	    demoMower.ControlledMove(1000);
	    width_travelled = 0;
	    currentState = TRANSITION_LEFT; // change state and phase here!
	  } else if (obstacle_detected_phase_three && currentPhase == PHASETHREE) {
	    //	    ROS_INFO("************STOPPING");!
	    demoMower.ControlledMove(1000);
	    width_travelled = 0;
	    currentState = TRANSITION_RIGHT; // change state and phase here
	  }
	} else {
	  currentState = TURNING;
	}
	return;
      }
      //      ROS_INFO("Destination not yet reached.");
      // if we haven't travelled the required distance, adjust our bearing
      // with some frequency
      if (ekf_callback_counter == PID_FREQUENCY) {
	demoMower.WhatIsCurrentSpeed();
	geometry_msgs::Vector3 imuVector = QuaternionToYpr(msg);
	ekf_callback_counter = 0;
	double deviation = CalculateYawDeviation(imuVector.z);
	int correction_factor = (K_PID * deviation); 
	if (deviation > 0) {
	  ROS_INFO("veering left with factor %d", correction_factor);
	  demoMower.ControlledVeerLeft(correction_factor);
	} else {
	  correction_factor = -1 * correction_factor;
	  ROS_INFO("veering right with factor %d", correction_factor);
	  demoMower.ControlledVeerRight(correction_factor);
	}
	//ROS_INFO("Straight deviation is  %f", deviation);
	//demoMower.WhatIsCurrentSpeed();
      }

      // LOGGING CRAP
      if (false) {
	geometry_msgs::Vector3 imuVector = QuaternionToYpr(msg);
	std::ofstream myfile;
	myfile.open("~/Project/ros_ws/dave/pose_ekf_simulation/output/controller.txt", std::ios::app);
	double secs = ros::Time::now().toSec();
	if (!myfile.is_open()) {
	  ROS_ERROR("RAW ENCODER FILE NOT OPEN!");
	}
	myfile.precision(20);
	myfile << secs << " " 
	       << imuVector.z << " "
	       << estimated_current_yaw << " "
	       << CalculateYawDeviation(imuVector.z) << " "
	       << "\n";      
	myfile.close();
      }
      // END LOGGING CRAP
  }
  
  void PoseCallback(const nav_msgs::Odometry &msg) {
    // done the first time
    if (!yaw_global_set) {
      SetUpGlobalTurningGoals(msg);
      yaw_global_set = true;
    }
    if (currentState == DRIVING) {
      DriveStraight(msg);
    } else if (currentState == TURNING) {
      ekf_callback_counter = 0;
      if(currentPhase == PHASEONE ||
	 currentPhase == PHASETWO) {
	RightTurn(msg, 1500, 1);
      } else if(currentPhase == PHASETHREE ||
		currentPhase == PHASEFOUR) {
	LeftTurn(msg, 1500, 1);
      }
    } else if (currentState == TRANSITION_LEFT) {
      LeftTurn(msg, 1500, 1);
    } else if (currentState == TRANSITION_RIGHT) {
      RightTurn(msg, 1500, 1);
    }
    //ROS_INFO(" ");
  }

  void ObstacleCallback(const std_msgs::UInt32 &msg) {
    // Don't detect obstacles on even (turning) phases
    ROS_INFO("Got a message from the cameras!");

    if (entered_last_stage) {
      if (msg.data > 0) {
	ROS_INFO("Obstacle seen in last stage. Stopping.");
	currentState = STOPPED;
	demoMower.stop();
	
      }
    } else {
      if (msg.data > 0) {
	ROS_INFO("NEED TO STOP -- IMPACT IMMINENT!");
	if ((currentPhase == PHASEONE || currentPhase == PHASETHREE) &&
	    currentState == DRIVING) {
	  distance_to_obstacle = CalculateDistanceTravelled(last_known_x,
							    last_known_y);
	  obstacle_detected = true;
	  if (currentPhase == PHASEONE) {
	    obstacle_detected_phase_one = true;
	    obstacle_detected_phase_three = false;
	  } else {
	    obstacle_detected_phase_one = false;
	    obstacle_detected_phase_three = true;
	  }
	  demoMower.stop();
	  currentState = TURNING;
	}
      }
    }
  }

  double CalculateDistanceTravelled(double x, double y) {
    std::cout.precision(15);
    double dx, dy;
    // delta x = current pos - starting pos
    dx = x-X;
    dy = y-Y;
    double value = sqrt((dx*dx) + (dy*dy));
    //ROS_INFO("Distance travelled: %f", value);
    return value;
  }

  double CalculateAmountTurned(double current) {
    std::cout.precision(15);
    return (current - initial_yaw);
  }

  void SetEncoderData(double x, double y) {
    X = x;
    Y = y;
  }

  void SetImuData(double value) {
    initial_yaw = value;
  }

  geometry_msgs::Vector3 QuaternionToYpr(const nav_msgs::Odometry msg) {
    geometry_msgs::Vector3 ypr;
    geometry_msgs::Quaternion q = msg.pose.pose.orientation;
    ypr.x = atan2(q.w*q.x+q.y*q.z, 0.5-q.x*q.x-q.y*q.y);
    ypr.y = asin(2*(q.w*q.y - q.x*q.z));
    ypr.z = atan2(q.w*q.z+q.x*q.y, 0.5-q.y*q.y-q.z*q.z);
    return ypr;
  }

  void ResetEncoderData() {
    X = 0;
    Y = 0;
    return;
  }

  void ResetImuData() {
    initial_yaw = 0;
    turnsMade = 0;
  }

  bool IsEncoderDataCleared() {
    return ((X == 0) && (Y == 0));
  }

  bool IsImuDataCleared() {
    return (initial_yaw == 0);
  }

  void StartMoving() {
    ekf_callback_counter = 0;
    width_travelled = 0;
    currentState = DRIVING;
    currentPhase = PHASEONE;
  }
private:  
  // Nodehandle
  ros::NodeHandle n;

  // Subscribers
  ros::Subscriber encoder_sub;
  ros::Subscriber obstacle_sub;

  enum RobotState { DRIVING, TURNING, STOPPED, TRANSITION_LEFT, TRANSITION_RIGHT };
  RobotState currentState;

  enum PhaseState { PHASEONE, PHASETWO, PHASETHREE, PHASEFOUR };
  PhaseState currentPhase;

  // The number of turns made.
  int turnsMade;

  // Encoder data
  long double X;
  long double Y;
  long double last_known_x;
  long double last_known_y;
  
  // IMU data for making each turn
  double initial_yaw;

  // IMU data for global turning
  bool yaw_global_set;
  double yaw_forward;
  double yaw_backward;
  double yaw_left;
  double yaw_right;
  double estimated_current_yaw;
  double absolute_beginning_yaw;
  double yaw_goal;

  DemoMower demoMower;

  // PID frequency counter
  int ekf_callback_counter;

  // variables for stopping conditions
  double width_travelled;
  double distance_to_travel;
  bool obstacle_detected_phase_one;
  bool obstacle_detected_phase_three;
  double distance_to_obstacle;
  bool obstacle_detected;
  bool entered_last_stage;
};

int main(int argc, char** argv) {
  static char *port = "/dev/arduino";
  // Initializing the node for the GPS
  ros::init(argc, argv, "sensortest");
  ros::NodeHandle nh_;

  ROS_INFO("Attempting to connect to arduino...");
  SensorDemo *p = new SensorDemo(nh_, port);
  ROS_INFO("Connected to arduino! Beginning demonstration!");
  p->ResetEncoderData();
  p->ResetImuData();
  p->StartMoving();

  ros::spin();
  return 0;
}
